#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include "gvcp.h"
#include <dicom/queue.h>

static inline GevStatusCode
gvcp_packet_get_status (GvcpPacket *packet)
{
    if (packet == NULL)
        return (GevStatusCode) 0;
    
    return (GevStatusCode) ntohs (packet->header.acknowledge.status);
}

static inline GvcpCommandType
gvcp_packet_get_command (GvcpPacket *packet)
{
	if (packet == NULL)
		return (GvcpCommandType) 0;

	return (GvcpCommandType) ntohs (packet->header.command.cmd_type);
}

static inline GvcpCommandType
gvcp_packet_get_acknowledge (GvcpPacket *packet)
{
	if (packet == NULL)
		return (GvcpCommandType) 0;

	return (GvcpPacket) ntohs (packet->header.acknowledge.ack_type);
}

static inline uint16_t
gvcp_packet_get_length (GvcpPacket *packet)
{
    if (packet == NULL)
        return 0;
        
    return (uint16_t) ntohs (packet->header.command.length);
}

static inline uint16_t
gvcp_packet_get_count (GvcpPacket *packet)
{
    if (packet == NULL)
        return 0;
        
    return (uint16_t) ntohs (packet->header.command.req_id);
}

static inline uint16_t
gvcp_packet_increase_count (uint16_t count)
{
	/* skip 0 */
	if (count == 0xffff)
		return 1;
	return count + 1;
}

static inline void
gvcp_packet_free (GvcpPacket *packet)
{
    if (packet)
        free (packet);
}

GvcpPacket *
gvcp_packet_new_discovery_cmd (uint16_t req_id)
{
    GvcpPacket *packet;
    
    packet = malloc (sizeof(GvcpHeader));
    
    if (!packet)
        return NULL;
    
    packet->header.command.flag = htons (GVCP_COMMAND_FLAG_MUST_ACK);
    packet->header.command.cmd_type = htons (GVCP_COMMAND_DISCOVERY_CMD);
    packet->header.command.length = 0;
    packet->header.command.req_id = htons (req_id);
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_discovery_ack (uint16_t status, uint16_t req_id, GvcpDataDiscoveryAck *ack)
{
    GvcpPacket *packet;
    GvcpDataDiscoveryAck *discovery_ack_payload;
    
    packet = malloc (sizeof(GvcpHeader) + sizeof(GvcpDataDiscoveryAck));
    
    if (!packet)
        return NULL;
    
    memset (packet, 0, sizeof(GvcpHeader) + sizeof(GvcpDataDiscoveryAck));
    
    packet->header.acknowledge.status = htons (status);
    packet->header.acknowledge.ack_type = htons (GVCP_COMMAND_DISCOVERY_ACK);
    packet->header.acknowledge.length = htons (sizeof(GvcpDataDiscoveryAck));
    packet->header.acknowledge.ack_id = htons (req_id);
    
    discovery_ack_payload = (GvcpDataDiscoveryAck *)&packet->payload;
    
    if (ack)
        memcpy (discovery_ack_payload, ack, sizeof(GvcpDataDiscoveryAck));
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_forceip_cmd (uint16_t req_id, GvcpDataForceIpCmd *cmd)
{
    GvcpPacket *packet;
    GvcpDataForceIpCmd *forceip_cmd_payload;
    
    packet = malloc (sizeof(GvcpHeader) + sizeof(GvcpDataForceIpCmd));
    
    if (!packet)
        return NULL;
    
    memset (packet, 0, sizeof(GvcpHeader) + sizeof(GvcpDataForceIpCmd));
    
    packet->header.command.flag = htons (GVCP_COMMAND_FLAG_MUST_ACK);
    packet->header.command.cmd_type = htons (GVCP_COMMAND_FORCEIP_CMD);
    packet->header.command.length = htons (sizeof(GvcpDataForceIpCmd));
    packet->header.command.req_id = htons (req_id);
    
    forceip_cmd_payload = (GvcpDataForceIpCmd *)&packet->payload;
    
    if (cmd)
        memcpy (forceip_cmd_payload, cmd, sizeof(GvcpDataForceIpCmd));
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_forceip_ack (uint16_t status, uint16_t req_id)
{
    GvcpPacket *packet;
    
    packet = malloc (sizeof(GvcpHeader));
    
    if (!packet)
        return NULL;
    
    packet->header.acknowledge.status = htons (status);
    packet->header.acknowledge.ack_type = htons (GVCP_COMMAND_FORCEIP_ACK);
    packet->header.acknowledge.length = 0;
    packet->header.acknowledge.ack_id = htons (req_id);
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_readreg_cmd (uint16_t req_id, uint32_t reg_amount, uint32_t *reg_addr)
{
    GvcpPacket *packet;
    uint32_t *readreg_cmd_payload;
    uint32_t max_amount = (GVCP_PACKET_DATA_SIZE_MAX / sizeof(uint32_t));
    uint32_t data_size = sizeof(uint32_t) * reg_amount;
    int count;
    
    if (reg_amount > max_amount)
        return NULL;
    
    packet = malloc (sizeof(GvcpHeader) + data_size);
    
    if (!packet)
        return NULL;
    
    memset (packet, 0, sizeof(GvcpHeader) + data_size);
    
    packet->header.command.flag = htons (GVCP_COMMAND_FLAG_MUST_ACK);
    packet->header.command.cmd_type = htons (GVCP_COMMAND_READREG_CMD);
    packet->header.command.length = htons (data_size);
    packet->header.command.req_id = htons (req_id);
    
    readreg_cmd_payload = (uint32_t *)&packet->payload;
    
    for (count = 0; count < reg_amount; count++)
        readreg_cmd_payload[count] = htonl (reg_addr[count]);
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_readreg_ack (uint16_t status, uint16_t req_id, uint32_t reg_amount, uint32_t *reg_data)
{
    GvcpPacket *packet;
    uint32_t *readreg_ack_payload;
    uint32_t max_amount = (GVCP_PACKET_DATA_SIZE_MAX / sizeof(uint32_t));
    uint32_t data_size = sizeof(uint32_t) * reg_amount;
    int count;
    
    if (reg_amount > max_amount)
        return NULL;
    
    packet = malloc (sizeof(GvcpHeader) + data_size);
    
    if (!packet)
        return NULL;
    
    memset (packet, 0, sizeof(GvcpHeader) + data_size);
    
    packet->header.acknowledge.status = htons (status);
    packet->header.acknowledge.ack_type = htons (GVCP_COMMAND_READREG_ACK);
    packet->header.acknowledge.length = htons (data_size);
    packet->header.acknowledge.ack_id = htons (req_id);
    
    readreg_ack_payload = (uint32_t *)&packet->payload;
    
    if (reg_data)
        for (count = 0; count < reg_amount; count++)
            readreg_ack_payload[count] = htonl (reg_data[count]);
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_writereg_cmd (uint16_t req_id, uint32_t reg_amount, uint32_t *reg_addr, uint32_t *reg_data)
{
    GvcpPacket *packet;
    uint32_t *writereg_cmd_payload;
    uint32_t max_amount = (GVCP_PACKET_DATA_SIZE_MAX / (sizeof(uint32_t) * 2));
    uint32_t data_size = sizeof(uint32_t) * 2 * reg_amount;
    int count, index;
    
    if (reg_amount > max_amount)
        return NULL;
    
    packet = malloc (sizeof(GvcpHeader) + data_size);
    
    if (!packet)
        return NULL;
    
    memset (packet, 0, sizeof(GvcpHeader) + data_size);
    
    packet->header.command.flag = htons (GVCP_COMMAND_FLAG_MUST_ACK);
    packet->header.command.cmd_type = htons (GVCP_COMMAND_WRITEREG_CMD);
    packet->header.command.length = htons (data_size);
    packet->header.command.req_id = htons (req_id);
    
    writereg_cmd_payload = (uint32_t *)&packet->payload;
    
    for (count = 0, index = 0; count < reg_amount; count++, index += 2) {
        readreg_ack_payload[index] = htonl (reg_addr[index]);
        readreg_ack_payload[index + 1] = htonl (reg_data[index]);
    }
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_writereg_ack (uint16_t status, uint16_t req_id, uint16_t index)
{
    GvcpPacket *packet;
    GvcpDataWriteRegAck *writereg_ack_payload;
    
    packet = malloc (sizeof(GvcpHeader) + sizeof(GvcpDataWriteRegAck));
    
    if (!packet)
        return NULL;
    
    memset (packet, 0, sizeof(GvcpHeader) + sizeof(GvcpDataWriteRegAck));
    
    packet->header.acknowledge.status = htons (status);
    packet->header.acknowledge.ack_type = htons (GVCP_COMMAND_WRITEREG_ACK);
    packet->header.acknowledge.length = htons (sizeof(GvcpDataWriteRegAck));
    packet->header.acknowledge.ack_id = htons (req_id);
    
    writereg_ack_payload = (GvcpDataWriteRegAck *)&packet->payload;
    
    writereg_ack_payload->index = htons (index);
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_readmem_cmd (uint16_t req_id, uint32_t address, uint16_t count)
{
    GvcpPacket *packet;
    GvcpDataReadMemCmd *readmem_cmd_payload;
    
    packet = malloc (sizeof(GvcpHeader) + sizeof(GvcpDataReadMemCmd));
    
    if (!packet || (count & 3))
        return NULL;
    
    memset (packet, 0, sizeof(GvcpHeader) + sizeof(GvcpDataReadMemCmd));
    
    packet->header.command.flag = htons (GVCP_COMMAND_FLAG_MUST_ACK);
    packet->header.command.cmd_type = htons (GVCP_COMMAND_READMEM_CMD);
    packet->header.command.length = htons (sizeof(GvcpDataReadMemCmd));
    packet->header.command.req_id = htons (req_id);
    
    readmem_cmd_payload = (GvcpDataReadMemCmd *)&packet->payload;
    
    readmem_cmd_payload->address = htonl (address);
    readmem_cmd_payload->count = htons (count);
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_readmem_ack (uint16_t status, uint16_t req_id, uint32_t address, uint16_t count, uint8_t *data)
{
    GvcpPacket *packet;
    GvcpDataReadMemAck *readmem_ack_payload;
    uint32_t data_size = sizeof(uint32_t) + count;
    uint32_t max_count = GVCP_PACKET_DATA_SIZE_MAX - sizeof(uint32_t);
    
    if (count > max_count || (count & 3))
        return NULL;
    
    packet = malloc (sizeof(GvcpHeader) + data_size);
    
    if (!packet)
        return NULL;
    
    memset (packet, 0, sizeof(GvcpHeader) + data_size);
    
    packet->header.acknowledge.status = htons (status);
    packet->header.acknowledge.ack_type = htons (GVCP_COMMAND_READMEM_ACK);
    packet->header.acknowledge.length = htons (data_size);
    packet->header.acknowledge.ack_id = htons (req_id);
    
    readmem_ack_payload = (GvcpDataReadMemAck *)&packet->payload;
    
    readmem_ack_payload->address = htonl (address);
    if (data)
        memcpy (&readmem_ack_payload->data, data, count);
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_writemem_cmd (uint16_t req_id, uint32_t address, uint16_t count, uint8_t *data)
{
    GvcpPacket *packet;
    GvcpDataWriteMemCmd *writemem_cmd_payload;
    uint32_t data_size = sizeof(uint32_t) + count;
    uint32_t max_count = GVCP_PACKET_DATA_SIZE_MAX - sizeof(uint32_t);
    
    if (count > max_count || (count & 3))
        return NULL;
    
    packet = malloc (sizeof(GvcpHeader) + data_size);
    
    if (!packet)
        return NULL;
    
    memset (packet, 0, sizeof(GvcpHeader) + data_size);
    
    packet->header.command.flag = htons (GVCP_COMMAND_FLAG_MUST_ACK);
    packet->header.command.cmd_type = htons (GVCP_COMMAND_WRITEMEM_CMD);
    packet->header.command.length = htons (data_size);
    packet->header.command.req_id = htons (req_id);
    
    writemem_cmd_payload = (GvcpDataWriteMemCmd *)&packet->payload;
    
    writemem_cmd_payload->address = htonl (address);
    memcpy (&writemem_cmd_payload->data, data, count);
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_writemem_ack (uint16_t status, uint16_t req_id, uint16_t index)
{
    GvcpPacket *packet;
    GvcpDataWriteMemAck *writemem_ack_payload;
    
    packet = malloc (sizeof(GvcpHeader) + sizeof(GvcpDataWriteMemAck));
    
    if (!packet)
        return NULL;
    
    memset (packet, 0, sizeof(GvcpHeader) + sizeof(GvcpDataWriteMemAck));
    
    packet->header.acknowledge.status = htons (status);
    packet->header.acknowledge.ack_type = htons (GVCP_COMMAND_WRITEMEM_ACK);
    packet->header.acknowledge.length = htons (sizeof(GvcpDataWriteMemAck));
    packet->header.acknowledge.ack_id = htons (req_id);
    
    writemem_ack_payload = (GvcpDataWriteMemAck *)&packet->payload;
    
    writemem_ack_payload->index = htons (index);
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_pktresend_cmd (uint16_t req_id, uint16_t stream_channel_index, uint16_t block_id, 
                               uint32_t first_packet_id, uint32_t last_packet_id)
{
    GvcpPacket *packet;
    GvcpDataPktResendCmd *pktresend_cmd_payload;
    
    packet = malloc (sizeof(GvcpHeader) + sizeof(GvcpDataPktResendCmd));
    
    if (!packet)
        return NULL;
    
    memset (packet, 0, sizeof(GvcpHeader) + sizeof(GvcpDataPktResendCmd));
    
    packet->header.command.flag = htons (GVCP_COMMAND_FLAG_CLEAR);
    packet->header.command.cmd_type = htons (GVCP_COMMAND_PACKET_RESEND_CMD);
    packet->header.command.length = htons (sizeof(GvcpDataPktResendCmd));
    packet->header.command.req_id = htons (req_id);
    
    pktresend_cmd_payload = (GvcpDataPktResendCmd *)&packet->payload;
    
    pktresend_cmd_payload->stream_channel_index = htons (stream_channel_index);
    pktresend_cmd_payload->block_id = htons (block_id);
    pktresend_cmd_payload->first_packet_id = htonl (first_packet_id & 0x00ffffff);
    pktresend_cmd_payload->last_packet_id = htonl (last_packet_id & 0x00ffffff);
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_pending_ack (uint16_t status, uint16_t req_id, uint16_t time_to_completion)
{
    GvcpPacket *packet;
    GvcpDataPendingAck *pending_ack;
    
    packet = malloc (sizeof(GvcpHeader) + sizeof(GvcpDataPendingAck));
    
    if (!packet)
        return NULL;
    
    memset (packet, 0, sizeof(GvcpHeader) + sizeof(GvcpDataPendingAck));
    
    packet->header.acknowledge.status = htons (status);
    packet->header.acknowledge.ack_type = htons (GVCP_COMMAND_PENDING_ACK);
    packet->header.acknowledge.length = htons (sizeof(GvcpDataPendingAck));
    packet->header.acknowledge.ack_id = htons (req_id);
    
    pending_ack = (GvcpDataPendingAck *)&packet->payload;
    
    pending_ack->time_to_completion = htons (time_to_completion);
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_action_cmd (uint16_t req_id, uint32_t device_key, uint32_t group_key, uint32_t group_mask)
{
    GvcpPacket *packet;
    GvcpDataActionCmd *action_cmd;
    
    packet = malloc (sizeof(GvcpHeader) + sizeof(GvcpDataActionCmd));
    
    if (!packet)
        return NULL;
    
    memset (packet, 0, sizeof(GvcpHeader) + sizeof(GvcpDataActionCmd));
    
    packet->header.command.flag = htons (GVCP_COMMAND_FLAG_MUST_ACK);
    packet->header.command.cmd_type = htons (GVCP_COMMAND_ACTION_CMD);
    packet->header.command.length = htons (sizeof(GvcpDataActionCmd));
    packet->header.command.req_id = htons (req_id);
    
    action_cmd = (GvcpDataActionCmd *)&packet->payload;
    
    action_cmd->device_key = htonl (device_key);
    action_cmd->group_key = htonl (group_key);
    action_cmd->group_mask = htonl (group_mask);
    
    return packet;
}

GvcpPacket *
gvcp_packet_new_action_ack (uint16_t status, uint16_t req_id)
{
    GvcpPacket *packet;
    
    packet = malloc (sizeof(GvcpHeader));
    
    if (!packet)
        return NULL;
    
    packet->header.acknowledge.status = htons (status);
    packet->header.acknowledge.ack_type = htons (GVCP_COMMAND_ACTION_ACK);
    packet->header.acknowledge.length = 0;
    packet->header.acknowledge.ack_id = htons (req_id);
    
    return packet;
}

void *
gvcp_packet_receiver (queue_t *packet_queue)
{
    while (1) {

    }
}

void *
gvcp_packet_handler (queue_t *packet_queue)
{
    GvcpPacket *packet;

    while (1) {
        /* Wait until new packet arrive */
        packet = queue_pop_wait (packet_queue);

        if ((packet->command.flag & 0xff00) == GVCP_COMMAND_CODE) {
            /* This is a command packet */
            switch (ntohs (packet->header.command.cmd_type)) {
                case GVCP_COMMAND_DISCOVERY_CMD:
                    break;
                case GVCP_COMMAND_FORCEIP_CMD:
                    break;
                case GVCP_COMMAND_PACKET_RESEND_CMD:
                    break;
                case GVCP_COMMAND_READREG_CMD:
                    break;
                case GVCP_COMMAND_WRITEREG_CMD:
                    break;
                case GVCP_COMMAND_READMEM_CMD:
                    break;
                case GVCP_COMMAND_WRITEMEM_CMD:
                    break;
                case GVCP_COMMAND_EVENT_CMD:
                    break;
                case GVCP_COMMAND_EVENTDATA_CMD:
                    break;
                case GVCP_COMMAND_ACTION_CMD:
                    break;
            }
        }
        else {
            /* This is an acknowledge packet */
            switch (ntohs (packet->header.acknowledge.ack_type)) {
                case GVCP_COMMAND_DISCOVERY_ACK:
                    break;
                case GVCP_COMMAND_FORCEIP_ACK:
                    break;
                case GVCP_COMMAND_PACKET_RESEND_ACK:
                    break;
                case GVCP_COMMAND_READREG_ACK:
                    break;
                case GVCP_COMMAND_WRITEREG_ACK:
                    break;
                case GVCP_COMMAND_READMEM_ACK:
                    break;
                case GVCP_COMMAND_WRITEMEM_ACK:
                    break;
                case GVCP_COMMAND_PENDING_ACK:
                    break;
                case GVCP_COMMAND_EVENT_ACK:
                    break;
                case GVCP_COMMAND_EVENTDATA_ACK:
                    break;
                case GVCP_COMMAND_ACTION_ACK:
                    break;
            }
        }
    }
}

